/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file aps.cxx
	\brief class implemenation for the Brock Tech. Antenna Pointing System

                    \author Maciej Stachura
             $Date: 2010/03/31 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>

#include "aps.h"

  /*<-------Defines--------->*/
  /*<------End Defines------>*/

  /*<---Global Variables---->*/
    extern bool verbose;
  /*<-End Global Variables-->*/

Aps::Aps(String name) : ThreadedPipe(name)
{
}

void Aps::update() 
{
  int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

  FD_ZERO(&readFDs);	                    		// zero read fd set
  FD_ZERO(&writeFDs);	                    		// zero write fd set
  FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
  FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
  FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

  maxFD = fifo_FDs[INBOARD_RX];
  if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
  if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

  timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

  if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
    if( val != 0 ) {
      if (errno != EINTR) {
        perror("APS::update() select");
        execRate = -1;
      } else
        cout << getName() << "::update() - select interrupted" << endl;
    }
  }
  else {
    if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
      // simple pipe
      int n = 0;
      n=readFifo(INBOARD_RX);
      if (n>0){
        uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length != fifo_msg.size) {
					cout << getName() << "::update() - error, length of command != length of buffer" << endl
						<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

					if( length < 1000 && length <= fifo_msg.size ){
						if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
							cout << "\tCMD: is valid" << endl;
						else
							cout << "\tCMD: is NOT valid" << endl;
					}
					return;
				}

				Command cmd(fifo_msg.data);

				if( !cmd.isValid() )
					cout << getName() << "::update() - error, invalid command" << endl << cmd ;
        else {
					Client * a_client;
					a_client = shmPtr->clients.getClient(fifo_msg.src);
					if(a_client != NULL) {

						uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

						switch(cmd.cmd() ) {
              case CMD_ANT_TRACK:
              {
                AntPoint_t * point_telem = (AntPoint_t*)dataPtr;

                float azimuth, elevation;
                // Convert to an 8 bit packet for the APS
                if (point_telem->mode == OMNI) {
									azimuth = 0; elevation = 90;} // Straight Up.
                else if (point_telem->mode == HOME) {
									azimuth = 0; elevation = 0;} //FIXME
                else {
									azimuth = point_telem->azimuth;
									elevation = point_telem->elevation;
								}

								// fix for reorienting APS antenna -- elstonj -- //
								azimuth = azimuth-180.0;                         //
								azimuth = fmod(azimuth,360.0);                   //
								if(azimuth < 0) azimuth = 360.0 + azimuth;       //
								elevation = 90-elevation;                        //
								// fix for reorienting APS antenna -- elstonj -- //
								
								elevation = round(elevation);
								azimuth = round(azimuth);

                int az100 = (int)azimuth/100;
                int az10  = (int)(azimuth - az100*100)/10;
                int az1   = (int)(azimuth - az100*100 - az10*10);
                int el10  = (int)elevation/10;
                int el1   = (int)(elevation - el10*10);
                sprintf((char*)fifo_msg.data, "!%d%d%d%d%d", az100, az10, az1, el10, el1);

								fifo_msg.size = 6;
								Client * me = shmPtr->clients.getClient(0);
								fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr;
								fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;

                // Output to the APS
								if(FD_ISSET(fifo_FDs[OUTBOARD_TX], &writeFDs)) {
									writeFifo(OUTBOARD_TX);
								}
              }
            }
          }
        }
      }
    }
  }
}
